/*
 * @Author: luoqi
 * @Date: 2021-04-27 19:20:38
 * @ Modified by: luoqi
 * @ Modified time: 2025-03-21 08:58
 */

#include "pid.h"

static inline qfp_t _abs(qfp_t x)
{
    return x > 0 ? x : -x;
}

static inline qfp_t update_pid_output(PidObj *pid)
{
    if(pid->omax != PID_UNLIMITED) {
        if(pid->y_k > pid->omax) {
            pid->y_k = pid->omax;
        } else if(pid->y_k < -pid->omax) {
            pid->y_k = -pid->omax;
        }
    }
    pid->y_k1 = pid->y_k;
    return pid->y_k;
}

int pid_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t omax)
{
    if(!pid) {
        return -1;
    }
    pid->delta_k = 0;
    pid->e_k1 = 0;
    pid->de_k1 = 0;
    pid->de_k2 = 0;
    pid->y_k1 = 0;
    pid->y_k = 0;
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->alpha = 1;
    pid->kaw = ki;
    pid->nlo_k1 = 0;
    pid->omax = omax;
    return 0;
}

int pid_param_set(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd)
{
    if(!pid) {
        return -1;
    }
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    return 0;
}

qfp_t pid_calc(PidObj *pid, qfp_t err, qfp_t dt)
{
    if(!pid || dt < 0) {
        return -1;
    }
    qfp_t de_k = (dt != PID_UNLIMITED) ? (err - pid->e_k1) / dt : err - pid->e_k1;
    pid->delta_k = pid->kp * (err - pid->e_k1)
        + pid->ki * err
        + pid->kd * (de_k - pid->de_k1);

    pid->de_k1 = de_k;
    pid->e_k1 = err;
    pid->y_k = pid->y_k1 + pid->delta_k;

    return update_pid_output(pid);
}

qfp_t pid_aw_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t kaw, qfp_t omax)
{
    if(!pid) {
        return -1;
    }
    pid_init(pid, kp, ki, kd, omax);
    pid->kaw = kaw;
    return 0;
}

qfp_t pid_aw_calc(PidObj *pid, qfp_t err, qfp_t dt)
{
    if(!pid || dt < 0) {
        return -1;
    }

    // Calculate the derivative term (prevent dt from being 0 or special value)
    qfp_t de_k = (dt != PID_UNLIMITED) ? (err - pid->e_k1) / dt : err - pid->e_k1;

    // Calculate the p_term and derivative terms
    qfp_t p_term = pid->kp * (err - pid->e_k1);             // Proportional term
    qfp_t d_term = pid->kd * (de_k - pid->de_k1);           // Derivative term
    
    pid->delta_k = p_term + d_term + pid->ki * err - pid->kaw * (pid->nlo_k1 - pid->y_k1);
    pid->y_k = pid->y_k1 + pid->delta_k;
    pid->nlo_k1 = pid->y_k;
    pid->de_k1 = de_k;
    pid->e_k1 = err;

    return update_pid_output(pid);
}

int pid_reset(PidObj *pid)
{
    if(!pid) {
        return -1;
    }

    pid->y_k = 0;
    pid->y_k1 = 0;
    pid->delta_k = 0;
    pid->nlo_k1 = 0;
    pid->e_k1 = 0;
    pid->de_k1 = 0;
    pid->de_k2 = 0;
    pid->ey_k1 = 0;
    pid->ey_k2 = 0;
    return 0;
}

int pid_calc_reset(PidObj *pid)
{
    if(!pid) {
        return -1;
    }

    pid->delta_k = 0;
    pid->e_k1 = 0;
    pid->nlo_k1 = 0;
    pid->de_k1 = 0;
    pid->de_k2 = 0;
    pid->ey_k1 = 0;
    pid->ey_k2 = 0;
    return 0;
}
